/*
 * eyebrowsHandler.cpp
 *
 *  Created on: Nov 28, 2012
 *      Author: michiel
 */

#include "emotion/eyebrow_handler.h"

// constructor
EyebrowsHandler::EyebrowsHandler()
{
	_eyebrows_pub = nh_.advertise<comm::eyebrows>(Topic::MOTOR_BROW, 1);
}

// destructor
EyebrowsHandler::~EyebrowsHandler() { }

void EyebrowsHandler::init() {

}

void EyebrowsHandler::spin() {
	ros::Rate loop_rate(30);

	while (ros::ok()) {
		if (emoteOk()) {
			processEmotion();
			publish();
		}

		ros::spinOnce();
		loop_rate.sleep();
	}
}

/**
 * Set the eyebrows according to the emotion.
 */
void EyebrowsHandler::processEmotion() {
	switch (getEmotion()) {
	case EMOTION_HAPPY:
		setEyebrowState(EYEBROWS_UP);
		break;
	case EMOTION_BORED:
		setEyebrowState(EYEBROWS_DOWN);
		break;
	case EMOTION_SAD:
		setEyebrowState(EYEBROWS_DOWN);
		break;
	default:
		setEyebrowState(EYEBROWS_NEUTRAL);
		setEyebrowAngle(0.25);
		break;
	}
}

/**
 * Publish the state of the eybrows.
 */
void EyebrowsHandler::publish()
{
	_eyebrows_pub.publish(_eyebrows_state);
}

/**
 * Set the state of the eyebrows.
 * @state	state of the eyebrow.
 */
void EyebrowsHandler::setEyebrowState(int state)
{
	_eyebrows_state.state = state;
}

/**
 * Set the eyebrow angle.
 * @angle	angle of the eyebrow (only used for rviz)
 */
void EyebrowsHandler::setEyebrowAngle(float angle)
{
	_eyebrows_state.angle = angle;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "eyebrowsHandlerEmotion");

	EyebrowsHandler handler;
	handler.init();
	handler.spin();
}


